﻿using System;
using System.Collections.Generic;
using static System.Math;

namespace GeoFits
{
    /// <summary>
    /// 圆柱拟合
    /// </summary>
    public static class CylinderFit
    {        
        /// <summary>
        /// 圆柱拟合(精度高、速度慢)
        /// </summary>
        /// <param name="pts">要进行拟合的点</param>
        /// <param name="P">权阵</param>
        /// <param name="radius">圆柱半径：不指定或指定为0时自动忽略。</param>
        /// <param name="config">是否使用抗差最小二乘法及抗差最小二乘法相关配置</param>
        /// <returns>拟合得到的圆柱</returns>
        /// <remarks>此方法先使用了FitInFastMode拟合作为初始值，然后进行进一步拟合</remarks>
        public static FitResult Fit(List<Point> pts, Matrix P = null, double radius = 0, RLSConfig config = null)
        {
            Cylinder cy0 = FitInFastMode(pts, P, radius, config).ResultGeometry as Cylinder;
            Quaternion qt = Quaternion.getQuaternion(cy0.Direct, new Vector(1, 0, 0));
            List<Point> ptAftRo = Quaternion.Rotation(pts, qt);

            var centerAftRo = Quaternion.Rotation(cy0.Center, qt);            

            FitResult result = new FitResult();
            Cylinder retCy;
            if (radius <= 0)
            {
                //表明此时不约束半径
                Matrix m0 = new Matrix(new double[,] { { centerAftRo.Y }, { centerAftRo.Z }, { 0 }, { 0 }, { cy0.Radius } });
                Matrix retM = FitPattern.LinearizedModule(ptAftRo, m0, CylinderFitFunctionModule, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, null, null, config);
                retCy = new Cylinder(new Point(0, retM[0, 0], retM[1, 0]), new Vector(1, retM[2, 0], retM[3, 0]), Math.Abs(retM[4, 0]));
            }
            else
            {
                //表明此时约束半径
                //注意输入的m0一定要与输出的m1维度相同
                Matrix m0 = new Matrix(new double[,] { { ptAftRo[0].Y }, { ptAftRo[0].Z }, { 0 }, { 0 } });
                Matrix retM = FitPattern.LinearizedModule(ptAftRo, m0, CylinderFitFunctionModule_WithRadius, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, null, null, config, radius);
                retCy = new Cylinder(new Point(0, retM[0, 0], retM[1, 0]), new Vector(1, retM[2, 0], retM[3, 0]), radius);
            }
            retCy.Center = Quaternion.Rotation(retCy.Center, qt.Inverse());
            retCy.Direct = Quaternion.Rotation(retCy.Direct, qt.Inverse());
            retCy = setPointOnMiddleOfCylinder(pts, retCy);
            result.ResultGeometry = retCy;
            return result;
        }

        /// <summary>
        /// 圆柱拟合(精度低、速度快，适合作初始值)
        /// </summary>
        /// <param name="pts">要进行拟合的点</param>
        /// <param name="P">权阵</param>
        /// <param name="radius">圆柱半径：不指定或指定为0时自动忽略。</param>
        /// <param name="config">是否使用抗差最小二乘法及抗差最小二乘法相关配置</param>
        /// <returns>拟合得到的圆柱</returns>
        public static FitResult FitInFastMode(List<Point> pts, Matrix P = null, double radius = 0, RLSConfig config = null)
        {
            Quaternion qt = Quaternion.getQuaternion(FitHelper.getDiffVector(pts), new Vector(1, 0, 0));
            List<Point> ptAftRo = Quaternion.Rotation(pts, qt);

            FitResult result = new FitResult();
            Cylinder retCy;
            if (radius <= 0)
            {
                //表明此时不约束半径
                Matrix m0 = new Matrix(new double[,] { { ptAftRo[0].Y }, { ptAftRo[0].Z }, { 0 }, { 0 }, { 1 } });
                Matrix retM = FitPattern.LinearizedModule(ptAftRo, m0, CylinderFitFunctionModule_InFastMode, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, null, null, config);
                retCy = new Cylinder(new Point(0, retM[0, 0], retM[1, 0]), new Vector(1, retM[2, 0], retM[3, 0]), Math.Abs(retM[4, 0]));
            }
            else
            {
                //表明此时约束半径
                //注意输入的m0一定要与输出的m1维度相同
                Matrix m0 = new Matrix(new double[,] { { ptAftRo[0].Y }, { ptAftRo[0].Z }, { 0 }, { 0 } });
                Matrix retM = FitPattern.LinearizedModule(ptAftRo, m0, CylinderFitFunctionModule_WithRadiusInFastMode, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, null, null, config, radius);
                retCy = new Cylinder(new Point(0, retM[0, 0], retM[1, 0]), new Vector(1, retM[2, 0], retM[3, 0]), radius);
            }
            retCy.Center = Quaternion.Rotation(retCy.Center, qt.Inverse());
            retCy.Direct = Quaternion.Rotation(retCy.Direct, qt.Inverse());
            retCy = setPointOnMiddleOfCylinder(pts, retCy);
            result.ResultGeometry = retCy;
            return result;
        }

        #region 私有函数
        /// <summary>
        /// 圆柱拟合完成后，将点移动到圆柱约中心位置
        /// </summary>
        /// <param name="pts">拟合用点</param>
        /// <param name="retCy">移动点后的圆柱</param>
        /// <returns></returns>
        private static Cylinder setPointOnMiddleOfCylinder(List<Point> pts, Cylinder retCy)
        {
            double allx = 0, ally = 0, allz = 0;
            foreach (var pt in pts)
            {
                allx += pt.X;
                ally += pt.Y;
                allz += pt.Z;
            }
            Vector dir = retCy.Direct;
            if (Abs(dir.X) > Abs(dir.Y) && Abs(dir.X) > Abs(dir.Z))
            {
                double meanx = allx / pts.Count;
                double multiple = (meanx - retCy.Center.X) / dir.X;
                double newy = retCy.Center.Y + multiple * dir.Y;
                double newz = retCy.Center.Z + multiple * dir.Z;
                retCy.Center = new Point(meanx, newy, newz);
            }
            else if (Abs(dir.Y) > Abs(dir.X) && Abs(dir.Y) > Abs(dir.Z))
            {
                double meany = ally / pts.Count;
                double multiple = (meany - retCy.Center.Y) / dir.Y;
                double newx = retCy.Center.X + multiple * dir.X;
                double newz = retCy.Center.Z + multiple * dir.Z;
                retCy.Center = new Point(newx, meany, newz);
            }
            else
            {
                double meanz = allz / pts.Count;
                double multiple = (meanz - retCy.Center.Z) / dir.Z;
                double newy = retCy.Center.Y + multiple * dir.Y;
                double newx = retCy.Center.X + multiple * dir.X;
                retCy.Center = new Point(newx, newy, meanz);
            }
            return retCy;
        }

        private static void CylinderFitFunctionModule_WithRadiusInFastMode(List<Point> pts, out Matrix A, out Matrix L, Matrix m0, double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(n, 4);
            L = new Matrix(n, 1);
            //限制条件
            if (conditions.Length < 1)
            {
                throw new ParametersWrongException("请指定壁厚！");
            }
            double radius = conditions[0];
            double length = 1 + m0[2, 0] * m0[2, 0] + m0[3, 0] * m0[3, 0];
            for (int i = 0; i < n; i++)
            {
                double dotProduct = pts[i].X + m0[2, 0] * (pts[i].Y - m0[0, 0]) + m0[3, 0] * (pts[i].Z - m0[1, 0]);
                double temp = pts[i].X * pts[i].X + (pts[i].Y - m0[0, 0]) * (pts[i].Y - m0[0, 0]) + (pts[i].Z - m0[1, 0]) * (pts[i].Z - m0[1, 0]) - radius * radius;

                A[i, 0] = 2 * m0[2, 0] * dotProduct + 2 * (m0[0, 0] - pts[i].Y) * length;
                A[i, 1] = 2 * m0[3, 0] * dotProduct + 2 * (m0[1, 0] - pts[i].Z) * length;
                A[i, 2] = 2 * (m0[0, 0] - pts[i].Y) * dotProduct + 2 * m0[2, 0] * temp;
                A[i, 3] = 2 * (m0[1, 0] - pts[i].Z) * dotProduct + 2 * m0[3, 0] * temp;

                L[i, 0] = dotProduct * dotProduct - length * temp;
            }
        }

        private static void CylinderFitFunctionModule_InFastMode(List<Point> pts, out Matrix A, out Matrix L, Matrix m0, double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(n, 5);
            L = new Matrix(n, 1);
            double length = 1 + m0[2, 0] * m0[2, 0] + m0[3, 0] * m0[3, 0];
            for (int i = 0; i < n; i++)
            {
                double dotProduct = pts[i].X + m0[2, 0] * (pts[i].Y - m0[0, 0]) + m0[3, 0] * (pts[i].Z - m0[1, 0]);
                double temp = pts[i].X * pts[i].X + (pts[i].Y - m0[0, 0]) * (pts[i].Y - m0[0, 0]) + (pts[i].Z - m0[1, 0]) * (pts[i].Z - m0[1, 0]) - m0[4, 0] * m0[4, 0];

                A[i, 0] = 2 * m0[2, 0] * dotProduct + 2 * (m0[0, 0] - pts[i].Y) * length;
                A[i, 1] = 2 * m0[3, 0] * dotProduct + 2 * (m0[1, 0] - pts[i].Z) * length;
                A[i, 2] = 2 * (m0[0, 0] - pts[i].Y) * dotProduct + 2 * m0[2, 0] * temp;
                A[i, 3] = 2 * (m0[1, 0] - pts[i].Z) * dotProduct + 2 * m0[3, 0] * temp;
                A[i, 4] = -2 * m0[4, 0] * length;

                L[i, 0] = dotProduct * dotProduct - length * temp;
            }
        }

        private static void CylinderFitFunctionModule_WithRadius(List<Point> pts, out Matrix A, out Matrix L, Matrix m0, double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(n, 5);
            L = new Matrix(n, 1);
            //限制条件
            if (conditions.Length < 1)
            {
                throw new ParametersWrongException("请指定壁厚！");
            }
            double radius = conditions[0];
            double a = Math.Sqrt(1 - m0[2, 0] * m0[2, 0] - m0[3, 0] * m0[3, 0]);
            for (int i = 0; i < n; i++)
            {
                double dotProduct = pts[i].X * a + m0[2, 0] * (pts[i].Y - m0[0, 0]) + m0[3, 0] * (pts[i].Z - m0[1, 0]);
                double temp = Math.Sqrt(pts[i].X * pts[i].X + (pts[i].Y - m0[0, 0]) * (pts[i].Y - m0[0, 0]) + (pts[i].Z - m0[1, 0]) * (pts[i].Z - m0[1, 0]) - dotProduct * dotProduct);

                A[i, 0] = (m0[2, 0] * dotProduct + m0[0, 0] - pts[i].Y) / temp;

                A[i, 1] = (m0[3, 0] * dotProduct + m0[1, 0] - pts[i].Z) / temp;
                A[i, 2] = -((-m0[2, 0] * pts[i].X / a) + pts[i].Y - m0[0, 0]) * dotProduct / temp;
                A[i, 3] = -((-m0[3, 0] * pts[i].X / a) + pts[i].Z - m0[1, 0]) * dotProduct / temp;
                A[i, 4] = -1;
                L[i, 0] = radius - temp;
            }
        }

        private static void CylinderFitFunctionModule(List<Point> pts, out Matrix A, out Matrix L, Matrix m0, double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(n, 5);
            L = new Matrix(n, 1);
            double a = Math.Sqrt(1 - m0[2, 0] * m0[2, 0] - m0[3, 0] * m0[3, 0]);
            for (int i = 0; i < n; i++)
            {
                double dotProduct = pts[i].X * a + m0[2, 0] * (pts[i].Y - m0[0, 0]) + m0[3, 0] * (pts[i].Z - m0[1, 0]);
                double temp = Math.Sqrt(pts[i].X * pts[i].X + (pts[i].Y - m0[0, 0]) * (pts[i].Y - m0[0, 0]) + (pts[i].Z - m0[1, 0]) * (pts[i].Z - m0[1, 0]) - dotProduct * dotProduct);

                A[i, 0] = (m0[2, 0] * dotProduct + m0[0, 0] - pts[i].Y) / temp;

                A[i, 1] = (m0[3, 0] * dotProduct + m0[1, 0] - pts[i].Z) / temp;
                A[i, 2] = -((-m0[2, 0] * pts[i].X / a) + pts[i].Y - m0[0, 0]) * dotProduct / temp;
                A[i, 3] = -((-m0[3, 0] * pts[i].X / a) + pts[i].Z - m0[1, 0]) * dotProduct / temp;
                A[i, 4] = -1;
                L[i, 0] = m0[4, 0] - temp;
            }
        }
        #endregion
    }    
}